/*!
 * @file rt_spi.h
 * @brief SPI communication to spine board
 */
#ifdef linux

#include <byteswap.h>
#include <math.h>
#include <pthread.h>
#include <stdio.h>
#include <string.h>

#include "rt/rt_spi.h"
#include <lcm/lcm-cpp.hpp>
#include <linux/spi/spidev.h>

unsigned char spi_mode          = SPI_MODE_3;
unsigned char spi_bits_per_word = 16;
unsigned int  spi_speed         = 18000000;
// uint8_t lsb = 0x01;

int spi_1_fd = -1;
int spi_2_fd = -1;

int spi_open();

static spine_cmd_t        g_spine_cmd;
static spine_data_t       g_spine_data;
static SpineCommandStruct g_spine_cmd_n;
static SpineDataStruct    g_spine_data_n;

spi_command_t spi_command_drv;
spi_data_t    spi_data_drv;
spi_torque_t  spi_torque;

pthread_mutex_t spi_mutex;

// MIT原版 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// const float max_torque[3]      = {17.f, 17.f, 26.f};// TODO CHECK WITH BEN
// const float wimp_torque[3]     = {6.f, 6.f, 6.f};   // TODO CHECK WITH BEN
// const float disabled_torque[3] = {0.f, 0.f, 0.f};

// only used for actual robot
// const float abad_side_sign[4] = {-1.f, -1.f, 1.f, 1.f};
// const float hip_side_sign[4]  = {-1.f, 1.f, -1.f, 1.f};
// const float knee_side_sign[4] = {-.6429f, .6429f, -.6429f, .6429f};

// only used for actual robot
// const float abad_offset[4] = {0.f, 0.f, 0.f, 0.f};
// const float hip_offset[4]  = {M_PI / 2.f, -M_PI / 2.f, -M_PI / 2.f, M_PI / 2.f};
// const float knee_offset[4] = {K_KNEE_OFFSET_POS, -K_KNEE_OFFSET_POS, -K_KNEE_OFFSET_POS, K_KNEE_OFFSET_POS};

// 小米 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// const float max_torque[3]      = {12.f, 12.f, 12.f};// TODO CHECK WITH BEN
// const float wimp_torque[3]     = {4.f, 4.f, 4.f};   // TODO CHECK WITH BEN
// const float disabled_torque[3] = {0.f, 0.f, 0.f};

// const float abad_side_sign[4] = {-1.f, -1.f, 1.f, 1.f};
// const float hip_side_sign[4]  = {-1.f, 1.f, -1.f, 1.f};
// const float knee_side_sign[4] = {-1.f, 1.f, -1.f, 1.f};

// const float abad_offset[4] = {-FRONT_ABAD_OFFSET, FRONT_ABAD_OFFSET, BACK_ABAD_OFFSET, -BACK_ABAD_OFFSET};
// const float hip_offset[4]  = {-FRONT_HIP_OFFSET, FRONT_HIP_OFFSET, -BACK_HIP_OFFSET, BACK_HIP_OFFSET};
// const float knee_offset[4] = {FRONT_KNEE_OFFSET, -FRONT_KNEE_OFFSET, BACK_KNEE_OFFSET, -BACK_KNEE_OFFSET};

// 灵足02 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
const float max_torque[3]      = {17.f, 17.f, 17.f};// TODO CHECK WITH BEN
const float wimp_torque[3]     = {6.f, 6.f, 6.f};   // TODO CHECK WITH BEN
const float disabled_torque[3] = {0.f, 0.f, 0.f};

const float abad_side_sign[4] = {-1.f, -1.f, 1.f, 1.f};
const float hip_side_sign[4]  = {-1.f, 1.f, -1.f, 1.f};
const float knee_side_sign[4] = {-1.f, 1.f, -1.f, 1.f};

const float abad_offset[4] = {-FRONT_ABAD_OFFSET, FRONT_ABAD_OFFSET, BACK_ABAD_OFFSET, -BACK_ABAD_OFFSET};
const float hip_offset[4]  = {-FRONT_HIP_OFFSET, FRONT_HIP_OFFSET, -BACK_HIP_OFFSET, BACK_HIP_OFFSET};
const float knee_offset[4] = {FRONT_KNEE_OFFSET, -FRONT_KNEE_OFFSET, BACK_KNEE_OFFSET, -BACK_KNEE_OFFSET};

/*!
 * Compute SPI message checksum
 * @param data : input
 * @param len : length (in 32-bit words)
 * @return
 */
uint32_t xor_checksum(uint32_t* data, size_t len)
{
    uint32_t t = 0;
    for(size_t i = 0; i < len; i++) t = t ^ data[i];
    return t;
}

/*!
 * Emulate the spi board to estimate the torque.
 */
void fake_spine_control(spi_command_t* cmd, spi_data_t* data, spi_torque_t* torque_out, int board_num)
{
    torque_out->tau_abad[board_num] = cmd->kp_abad[board_num] * (cmd->q_des_abad[board_num] - data->q_abad[board_num]) + cmd->kd_abad[board_num] * (cmd->qd_des_abad[board_num] - data->qd_abad[board_num]) + cmd->tau_abad_ff[board_num];
    torque_out->tau_hip[board_num]  = cmd->kp_hip[board_num] * (cmd->q_des_hip[board_num] - data->q_hip[board_num]) + cmd->kd_hip[board_num] * (cmd->qd_des_hip[board_num] - data->qd_hip[board_num]) + cmd->tau_hip_ff[board_num];
    torque_out->tau_knee[board_num] = cmd->kp_knee[board_num] * (cmd->q_des_knee[board_num] - data->q_knee[board_num]) + cmd->kd_knee[board_num] * (cmd->qd_des_knee[board_num] - data->qd_knee[board_num]) + cmd->tau_knee_ff[board_num];

    const float* torque_limits = disabled_torque;

    if(cmd->flags[board_num] & 0b1)
    {
        if(cmd->flags[board_num] & 0b10)
            torque_limits = wimp_torque;
        else
            torque_limits = max_torque;
    }

    if(torque_out->tau_abad[board_num] > torque_limits[0])
        torque_out->tau_abad[board_num] = torque_limits[0];
    if(torque_out->tau_abad[board_num] < -torque_limits[0])
        torque_out->tau_abad[board_num] = -torque_limits[0];

    if(torque_out->tau_hip[board_num] > torque_limits[1])
        torque_out->tau_hip[board_num] = torque_limits[1];
    if(torque_out->tau_hip[board_num] < -torque_limits[1])
        torque_out->tau_hip[board_num] = -torque_limits[1];

    if(torque_out->tau_knee[board_num] > torque_limits[2])
        torque_out->tau_knee[board_num] = torque_limits[2];
    if(torque_out->tau_knee[board_num] < -torque_limits[2])
        torque_out->tau_knee[board_num] = -torque_limits[2];
}

/*!
 * Initialize SPI
 */
void init_spi()
{
    // check sizes:
    size_t command_size = sizeof(spi_command_t);
    size_t data_size    = sizeof(spi_data_t);

    memset(&spi_command_drv, 0, sizeof(spi_command_drv));
    memset(&spi_data_drv, 0, sizeof(spi_data_drv));

    if(pthread_mutex_init(&spi_mutex, NULL) != 0)
        printf("[ERROR: RT SPI] Failed to create spi data mutex\n");

    if(command_size != K_EXPECTED_COMMAND_SIZE)
    {
        printf("[RT SPI] Error command size is %ld, expected %d\n", command_size, K_EXPECTED_COMMAND_SIZE);
    }
    else
        printf("[RT SPI] command size good\n");

    if(data_size != K_EXPECTED_DATA_SIZE)
    {
        printf("[RT SPI] Error data size is %ld, expected %d\n", data_size, K_EXPECTED_DATA_SIZE);
    }
    else
        printf("[RT SPI] data size good\n");

    printf("[RT SPI] Open\n");
    spi_open();
}

/*!
 * Open SPI device
 */
int spi_open()
{
    int rv   = 0;
    spi_1_fd = open("/dev/spidev0.0", O_RDWR);
    if(spi_1_fd < 0) perror("[ERROR] Couldn't open spidev 0.0");
    spi_2_fd = open("/dev/spidev0.1", O_RDWR);
    if(spi_2_fd < 0) perror("[ERROR] Couldn't open spidev 0.1");

    rv = ioctl(spi_1_fd, SPI_IOC_WR_MODE, &spi_mode);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_wr_mode (1)");

    rv = ioctl(spi_2_fd, SPI_IOC_WR_MODE, &spi_mode);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_wr_mode (2)");

    rv = ioctl(spi_1_fd, SPI_IOC_RD_MODE, &spi_mode);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_rd_mode (1)");

    rv = ioctl(spi_2_fd, SPI_IOC_RD_MODE, &spi_mode);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_rd_mode (2)");

    rv = ioctl(spi_1_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_wr_bits_per_word (1)");

    rv = ioctl(spi_2_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_wr_bits_per_word (2)");

    rv = ioctl(spi_1_fd, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_word);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_rd_bits_per_word (1)");

    rv = ioctl(spi_2_fd, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_word);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_rd_bits_per_word (2)");

    rv = ioctl(spi_1_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_wr_max_speed_hz (1)");
    rv = ioctl(spi_2_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_wr_max_speed_hz (2)");

    rv = ioctl(spi_1_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_rd_max_speed_hz (1)");
    rv = ioctl(spi_2_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
    if(rv < 0) perror("[ERROR] ioctl spi_ioc_rd_max_speed_hz (2)");

    // rv = ioctl(spi_1_fd, SPI_IOC_RD_LSB_FIRST, &lsb);
    // if (rv < 0) perror("[ERROR] ioctl spi_ioc_rd_lsb_first (1)");

    // rv = ioctl(spi_2_fd, SPI_IOC_RD_LSB_FIRST, &lsb);
    // if (rv < 0) perror("[ERROR] ioctl spi_ioc_rd_lsb_first (2)");
    return rv;
}

int spi_driver_iterations = 0;

/*!
 * convert spi command to spine_cmd_t
 */
void spi_to_spine_backup(spi_command_t* cmd, spine_cmd_t* spine_cmd, int leg_0)
{
    for(int i = 0; i < 2; i++)
    {
        // spine_cmd->q_des_abad[i] = (cmd->q_des_abad[i+leg_0] +
        // abad_offset[i+leg_0]) * abad_side_sign[i+leg_0]; spine_cmd->q_des_hip[i]
        // = (cmd->q_des_hip[i+leg_0] + hip_offset[i+leg_0]) *
        // hip_side_sign[i+leg_0]; spine_cmd->q_des_knee[i] =
        // (cmd->q_des_knee[i+leg_0] + knee_offset[i+leg_0]) /
        // knee_side_sign[i+leg_0];
        spine_cmd->q_des_abad[i] = (cmd->q_des_abad[i + leg_0] * abad_side_sign[i + leg_0]) + abad_offset[i + leg_0];
        spine_cmd->q_des_hip[i]  = (cmd->q_des_hip[i + leg_0] * hip_side_sign[i + leg_0]) + hip_offset[i + leg_0];
        spine_cmd->q_des_knee[i] = (cmd->q_des_knee[i + leg_0] / knee_side_sign[i + leg_0]) + knee_offset[i + leg_0];

        spine_cmd->qd_des_abad[i] = cmd->qd_des_abad[i + leg_0] * abad_side_sign[i + leg_0];
        spine_cmd->qd_des_hip[i]  = cmd->qd_des_hip[i + leg_0] * hip_side_sign[i + leg_0];
        spine_cmd->qd_des_knee[i] = cmd->qd_des_knee[i + leg_0] / knee_side_sign[i + leg_0];

        spine_cmd->kp_abad[i] = cmd->kp_abad[i + leg_0];
        spine_cmd->kp_hip[i]  = cmd->kp_hip[i + leg_0];
        spine_cmd->kp_knee[i] = cmd->kp_knee[i + leg_0];

        spine_cmd->kd_abad[i] = cmd->kd_abad[i + leg_0];
        spine_cmd->kd_hip[i]  = cmd->kd_hip[i + leg_0];
        spine_cmd->kd_knee[i] = cmd->kd_knee[i + leg_0];

        spine_cmd->tau_abad_ff[i] = cmd->tau_abad_ff[i + leg_0] * abad_side_sign[i + leg_0];
        spine_cmd->tau_hip_ff[i]  = cmd->tau_hip_ff[i + leg_0] * hip_side_sign[i + leg_0];
        spine_cmd->tau_knee_ff[i] = cmd->tau_knee_ff[i + leg_0] * knee_side_sign[i + leg_0];

        spine_cmd->flags[i] = cmd->flags[i + leg_0];
    }
    spine_cmd->checksum = xor_checksum((uint32_t*)spine_cmd, 32);
}

void spi_to_spine(spi_command_t* cmd, SpineCommandStruct* spine_cmd, int leg_0)
{
    LegCommandStruct* leg_cmd = &spine_cmd->right;
    for(int i = 0; i < 2; i++)
    {
        if(i == 1) leg_cmd = &spine_cmd->left;

        // if(fabs(cmd->q_des_abad[i + leg_0]) <= M_PI)// 0.61
        {
            leg_cmd->abad.a  = (cmd->q_des_abad[i + leg_0] * abad_side_sign[i + leg_0]) + abad_offset[i + leg_0];
            leg_cmd->abad.w  = cmd->qd_des_abad[i + leg_0] * abad_side_sign[i + leg_0];
            leg_cmd->abad.t  = cmd->tau_abad_ff[i + leg_0] * abad_side_sign[i + leg_0];
            leg_cmd->abad.kp = cmd->kp_abad[i + leg_0];
            leg_cmd->abad.kd = cmd->kd_abad[i + leg_0];
        }
        // if(fabs(cmd->q_des_hip[i + leg_0]) <= M_PI)// 2.7
        {
            leg_cmd->hip.a  = (cmd->q_des_hip[i + leg_0] * hip_side_sign[i + leg_0]) + hip_offset[i + leg_0];
            leg_cmd->hip.w  = cmd->qd_des_hip[i + leg_0] * hip_side_sign[i + leg_0];
            leg_cmd->hip.t  = cmd->tau_hip_ff[i + leg_0] * hip_side_sign[i + leg_0];
            leg_cmd->hip.kp = cmd->kp_hip[i + leg_0];
            leg_cmd->hip.kd = cmd->kd_hip[i + leg_0];
        }
        // if(fabs(cmd->q_des_knee[i + leg_0]) <= M_PI)// 2.56
        {
            leg_cmd->knee.a  = (cmd->q_des_knee[i + leg_0] / knee_side_sign[i + leg_0]) + knee_offset[i + leg_0];
            leg_cmd->knee.w  = cmd->qd_des_knee[i + leg_0] / knee_side_sign[i + leg_0];
            leg_cmd->knee.t  = cmd->tau_knee_ff[i + leg_0] * knee_side_sign[i + leg_0];
            leg_cmd->knee.kp = cmd->kp_knee[i + leg_0];
            leg_cmd->knee.kd = cmd->kd_knee[i + leg_0];
        }

        leg_cmd->flag = cmd->flags[i + leg_0];
    }
    spine_cmd->checksum = xor_checksum((uint32_t*)spine_cmd, 32);
}

/*!
 * convert spine_data_t to spi data
 */
void spine_to_spi_backup(spi_data_t* data, spine_data_t* spine_data, int leg_0)
{
    for(int i = 0; i < 2; i++)
    {
        data->q_abad[i + leg_0] = (spine_data->q_abad[i] - abad_offset[i + leg_0]) *
                                  abad_side_sign[i + leg_0];
        data->q_hip[i + leg_0] = (spine_data->q_hip[i] - hip_offset[i + leg_0]) *
                                 hip_side_sign[i + leg_0];
        data->q_knee[i + leg_0] = (spine_data->q_knee[i] - knee_offset[i + leg_0]) *
                                  knee_side_sign[i + leg_0];

        data->qd_abad[i + leg_0] =
                spine_data->qd_abad[i] * abad_side_sign[i + leg_0];
        data->qd_hip[i + leg_0] = spine_data->qd_hip[i] * hip_side_sign[i + leg_0];
        data->qd_knee[i + leg_0] =
                spine_data->qd_knee[i] * knee_side_sign[i + leg_0];

        data->flags[i + leg_0] = spine_data->flags[i];
    }

    uint32_t calc_checksum = xor_checksum((uint32_t*)spine_data, 32);
    if(calc_checksum != (uint32_t)spine_data->checksum)
        printf("SPI ERROR BAD CHECKSUM GOT 0x%hx EXPECTED 0x%hx\n", calc_checksum, spine_data->checksum);
}

void spine_to_spi(spi_data_t* data, SpineDataStruct* spine_data, int leg_0)
{
    uint32_t calc_checksum = xor_checksum((uint32_t*)spine_data, 32);
    if(calc_checksum != (uint32_t)spine_data->checksum)
    {
        printf("SPI ERROR BAD CHECKSUM GOT 0x%hx EXPECTED 0x%hx\n", calc_checksum, spine_data->checksum);
    }
    else
    {
        LegDataStruct* leg_data = &spine_data->right;
        for(int i = 0; i < 2; i++)
        {
            if(i == 1) leg_data = &spine_data->left;

            data->q_abad[i + leg_0] = (leg_data->abad.a - abad_offset[i + leg_0]) * abad_side_sign[i + leg_0];
            data->q_hip[i + leg_0]  = (leg_data->hip.a - hip_offset[i + leg_0]) * hip_side_sign[i + leg_0];
            data->q_knee[i + leg_0] = (leg_data->knee.a - knee_offset[i + leg_0]) * knee_side_sign[i + leg_0];

            data->qd_abad[i + leg_0] = leg_data->abad.w * abad_side_sign[i + leg_0];
            data->qd_hip[i + leg_0]  = leg_data->hip.w * hip_side_sign[i + leg_0];
            data->qd_knee[i + leg_0] = leg_data->knee.w * knee_side_sign[i + leg_0];

            data->tau_abad[i + leg_0] = leg_data->abad.t * abad_side_sign[i + leg_0];
            data->tau_hip[i + leg_0]  = leg_data->hip.t * hip_side_sign[i + leg_0];
            data->tau_knee[i + leg_0] = leg_data->knee.t / knee_side_sign[i + leg_0];

            data->temperature_abad[i + leg_0] = leg_data->abad.temperature;
            data->temperature_hip[i + leg_0]  = leg_data->hip.temperature;
            data->temperature_knee[i + leg_0] = leg_data->knee.temperature;

            data->status_abad[i + leg_0] = leg_data->abad.status;
            data->status_hip[i + leg_0]  = leg_data->hip.status;
            data->status_knee[i + leg_0] = leg_data->knee.status;

            data->flags[i + leg_0] = leg_data->flag;
        }
    }
}

/*!
 * send receive data and command from spine
 */
void spi_send_receive_backup(spi_command_t* command, spi_data_t* data)
{
    // update driver status flag
    spi_driver_iterations++;
    data->spi_driver_status = spi_driver_iterations << 16;

    // transmit and receive buffers
    uint16_t tx_buf[K_WORDS_PER_MESSAGE];
    uint16_t rx_buf[K_WORDS_PER_MESSAGE];

    for(int spi_board = 0; spi_board < 2; spi_board++)
    {
        // copy command into spine type:
        spi_to_spine_backup(command, &g_spine_cmd, spi_board * 2);

        // pointers to command/data spine array
        uint16_t* cmd_d  = (uint16_t*)&g_spine_cmd;
        uint16_t* data_d = (uint16_t*)&g_spine_data;

        // zero rx buffer
        memset(rx_buf, 0, K_WORDS_PER_MESSAGE * sizeof(uint16_t));

        // copy into tx buffer flipping bytes
        for(int i = 0; i < K_WORDS_PER_MESSAGE; i++)
            // tx_buf[i] = (cmd_d[i] >> 8) + ((cmd_d[i] & 0xff) << 8);
            tx_buf[i] = cmd_d[i];
        // tx_buf[i] = __bswap_16(cmd_d[i]);

        // each word is two bytes long
        size_t word_len = 2;// 16 bit word

        // spi message struct
        struct spi_ioc_transfer spi_message[1];

        // zero message struct.
        memset(spi_message, 0, 1 * sizeof(struct spi_ioc_transfer));

        // set up message struct
        for(int i = 0; i < 1; i++)
        {
            spi_message[i].bits_per_word = spi_bits_per_word;
            spi_message[i].cs_change     = 1;
            spi_message[i].delay_usecs   = 0;
            spi_message[i].len           = word_len * 66;
            spi_message[i].rx_buf        = (uint64_t)rx_buf;
            spi_message[i].tx_buf        = (uint64_t)tx_buf;
        }

        // do spi communication
        int rv = ioctl(spi_board == 0 ? spi_1_fd : spi_2_fd, SPI_IOC_MESSAGE(1), &spi_message);
        (void)rv;

        // flip bytes the other way
        for(int i = 0; i < 66; i++)
            // data_d[i] = (rx_buf[i] >> 8) + ((rx_buf[i] & 0xff) << 8);
            data_d[i] = rx_buf[i];
        // data_d[i] = __bswap_16(rx_buf[i]);

        // copy back to data
        spine_to_spi_backup(data, &g_spine_data, spi_board * 2);
    }
}

void spi_send_receive(spi_command_t* command, spi_data_t* data)
{
    // update driver status flag
    spi_driver_iterations++;
    data->spi_driver_status = spi_driver_iterations << 16;

    // transmit and receive buffers
    // uint16_t tx_buf[K_WORDS_PER_MESSAGE];
    // uint16_t rx_buf[K_WORDS_PER_MESSAGE];

    for(int spi_board = 0; spi_board < 2; spi_board++)
    {
        // copy command into spine type:
        spi_to_spine(command, &g_spine_cmd_n, spi_board * 2);

        // pointers to command/data spine array
        // uint16_t* cmd_d  = (uint16_t*)&g_spine_cmd_n;
        // uint16_t* data_d = (uint16_t*)&g_spine_data_n;

        // zero rx buffer
        // memset(rx_buf, 0, K_WORDS_PER_MESSAGE * sizeof(uint16_t));

        // spi message struct
        spi_ioc_transfer spi_message;
        memset(&spi_message, 0, 1 * sizeof(spi_ioc_transfer));
        spi_message.bits_per_word = spi_bits_per_word;
        spi_message.delay_usecs   = 0;
        spi_message.len           = 132;
        // spi_message.rx_buf        = (uint64_t)rx_buf;
        // spi_message.tx_buf        = (uint64_t)tx_buf;

        spi_message.rx_buf = (uint64_t)(uint16_t*)&g_spine_data_n;
        spi_message.tx_buf = (uint64_t)(uint16_t*)&g_spine_cmd_n;

        ioctl(spi_board == 0 ? spi_1_fd : spi_2_fd, SPI_IOC_MESSAGE(1), &spi_message);// SPI通讯

        // copy back to data
        spine_to_spi(data, &g_spine_data_n, spi_board * 2);

        // printf("spi_data: %d\n", spi_board);
        // for(int i = 0; i < 66; i++)
        // {
        //     printf("%04x ", rx_buf[i]);
        // }
    }
}

void showCommand(spi_command_t* cmd)
{
    // printf("spi_command: \n");
    for(int i = 0; i < 4; i++)
    {
        // cmd->kp_abad[i] = 500;
        // cmd->kp_hip[i]  = 500;
        // cmd->kp_knee[i] = 500;

        // cmd->kd_abad[i] = 0;
        // cmd->kd_hip[i]  = 0;
        // cmd->kd_knee[i] = 0;

        cmd->tau_abad_ff[i] = 0;
        cmd->tau_hip_ff[i]  = 0;
        cmd->tau_knee_ff[i] = 0;

        // if(fabs(cmd->q_des_abad[i]) > M_PI)
            // printf("abad[%d] a: %f w: %f t: %f kp: %f kd: %f flag: %d\n", i, cmd->q_des_abad[i], cmd->qd_des_abad[i], cmd->tau_abad_ff[i], cmd->kp_abad[i], cmd->kd_abad[i], cmd->flags[i]);
        // if(fabs(cmd->q_des_hip[i]) > M_PI)
            // printf("hip[%d]  a: %f w: %f t: %f kp: %f kd: %f flag: %d\n", i, cmd->q_des_hip[i], cmd->qd_des_hip[i], cmd->tau_hip_ff[i], cmd->kp_hip[i], cmd->kd_hip[i], cmd->flags[i]);
        // if(fabs(cmd->q_des_knee[i]) > M_PI)
            // printf("knee[%d] a: %f w: %f t: %f kp: %f kd: %f flag: %d\n", i, cmd->q_des_knee[i], cmd->qd_des_knee[i], cmd->tau_knee_ff[i], cmd->kp_knee[i], cmd->kd_knee[i], cmd->flags[i]);

        // if(cmd->kp_abad[i] != 80 && cmd->kd_abad[i] != 0)
        //     printf("kp_abad[%d] = %f kd_abad[%d] = %f\n", i, cmd->kp_abad[i], i, cmd->kd_abad[i]);
        // if(cmd->kp_hip[i] != 80 && cmd->kd_hip[i] != 0)
        //     printf("kp_hip[%d] = %f kd_hip[%d] = %f\n", i, cmd->kp_hip[i], i, cmd->kd_hip[i]);
        // if(cmd->kp_knee[i] != 80 && cmd->kd_knee[i] != 0)
        //     printf("kp_knee[%d] = %f kd_knee[%d] = %f\n", i, cmd->kp_knee[i], i, cmd->kd_knee[i]);
    }
}

void showData(spi_data_t* data)
{
    printf("spi_data: \n");
    for(int i = 0; i < 4; i++)
    {
        printf("abad[%d] a: %f w: %f t: %f status: %08X temp: %f flag: %d\n", i, data->q_abad[i], data->qd_abad[i], data->tau_abad[i], data->status_abad[i], data->temperature_abad[i], data->flags[i]);
        printf("hip[%d]  a: %f w: %f t: %f status: %08X temp: %f flag: %d\n", i, data->q_hip[i], data->qd_hip[i], data->tau_hip[i], data->status_hip[i], data->temperature_hip[i], data->flags[i]);
        printf("knee[%d] a: %f w: %f t: %f status: %08X temp: %f flag: %d\n", i, data->q_knee[i], data->qd_knee[i], data->tau_knee[i], data->status_knee[i], data->temperature_knee[i], data->flags[i]);
    }
    printf("\n");
}

/*!
 * Run SPI
 */
void spi_driver_run()
{
    // do spi board calculations
    for(int i = 0; i < 4; i++)
    {
        fake_spine_control(&spi_command_drv, &spi_data_drv, &spi_torque, i);
    }

    // in here, the driver is good
    pthread_mutex_lock(&spi_mutex);
    showCommand(&spi_command_drv);
    spi_send_receive(&spi_command_drv, &spi_data_drv);
    // showData(&spi_data_drv);
    pthread_mutex_unlock(&spi_mutex);
}

/*!
 * Get the spi command
 */
spi_command_t* get_spi_command()
{
    return &spi_command_drv;
}

/*!
 * Get the spi data
 */
spi_data_t* get_spi_data()
{
    return &spi_data_drv;
}

#endif
